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— , Abstract 

Motion planning problems have been studied by both the robotics and the controls research 
^ communities for a long time, and many algorithms have been developed for their solution. 

O ! Among them, incremental sampling-based motion planning algorithms, such as the Rapidly- 

exploring Random Trees (RRT), and the Probabilistic Road Maps (PRM) have become very 
popular recently, owing to their implementation simplicity and their advantages in handling 
high-dimensional problems. Although these algorithms work very well in practice, the quality 
of the computed solution is often not good, i.e., the solution can be far from the optimal one. A 
recent variation of RRT, namely the RRT* algorithm, bypasses this drawback of the traditional 
RRT algorithm, by ensuring asymptotic optimality as the number of samples tends to infinity. 
Nonetheless, the convergence rate to the optimal solution may still be slow. This paper presents 
^ a new incremental sampling-based motion planning algorithm based on Rapidly-exploring Ran- 

O dom Graphs (RRG), denoted RRT # (RRT "sharp") which also guarantees asymptotic optimal- 

ity but, in addition, it also ensures that the constructed spanning tree of the geometric graph 
y—^ is consistent after each iteration. In consistent trees, the vertices which have the potential to 

be part of the optimal solution have the minimum cost-come- value. This implies that the best 
possible solution is readily computed if there are some vertices in the current graph that are 
already in the goal region. Numerical results compare with the RRT* algorithm. 

Keywords: optimal motion planning, RRT, RRG, RRT , RRT*, vertex consistency, consistent tree. 

o 

1 Introduction 

. ^ Motion planning problems are crucial for the realization of truly autonomous vehicles and robots. 

^ Many approaches have been proposed in the literature (see for example, the excellent books by 

LaValle [15] and Choset et al [1]). A bottleneck in most motion planning problems, especially 
those involving systems with high state dimensionality, is the computational overhead associated 
with discretizing (i.e., gridding) the state space. Hence, deterministic searches [5] are impractical 
for high dimensional state spaces. Probabilistic roadmap methods [2, 10, 22, 11], [1, Ch. 7], as 
well as methods that use rapidly exploring random trees (RRTs) [16, 17, 6, 3, 21], are among the 
most popular. They can address the vehicle's kinematic and dynamic constraints during motion 
planning in high dimensional state spaces. In these methods, random samples of the obstacle-free 
space are connected to each other by feasible trajectories, and the resulting graph is searched for a 
sequence of connected samples from the initial state to the goal state. Sampling-based algorithms 
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require efficient low-level collision detection and trajectory planning algorithms to find collision- free 
trajectories between different samples [17]. 

Incremental sampling-based algorithms were first proposed by Kavraki during the late 1990s. 
The so-called Probabilistic Road Map (PRM) was successfully implemented to solve multi-query 
motion planning problems and gained a lot of attention, both in industry and academia [12]. In 
PRM a graph of the environment is constructed by taking random samples from the configuration 
space of the robot and testing them to determine whether they belong to the free space. The PRM 
algorithm uses a local planner that attempts to find a feasible path between the sampled points. 
Once a reasonable graph is constructed, the initial and the goal states are added to the graph, and 
the optimal path is computed using a graph search algorithm. 

Another important class of incremental sampled-based motion planning algorithm is the Rapidly- 
exploring Random Tree (RRT) and its numerous variants [17]. RRTs have achieved great success 
in solving single-query motion planning problems in many real-time applications. However, the 
quality of RRT-based algorithms is often poor (i.e., highly suboptimal). As a result, a lot of ef- 
fort has been devoted to the development of heuristic techniques in order to refine the quality of 
the solution obtained from RRTs. However, it has been recently shown that the best path re- 
turned by RRTs when the algorithm converges is almost always (i.e., with probability one) far from 
optimal [9]. This has renewed the interest to develop incremental sampled-based algorithms for 
motion-planning problems with optimality guarantees. In [7] the authors proposed the Rapidly- 
exploring Random Graphs (RRG) algorithm, which has asymptotic optimality properties, that is, 
it ensures that the optimal path will be found as the number of samples tends to infinity. Based on 
RRG, the same authors later proposed a new algorithm, namely RRT* that extracts a tree from 
the graph constructed by RRG [8, 9]. 

In this paper we present a new incremental sampling-based motion planning algorithm based on 
RRG, denoted RRT* (RRT "sharp"), which also guarantees asymptotic optimality but, in addition, 
it also ensures that, at each step, the constructed spanning tree of the graph is consistent. Vertex 
consistency (see Section 2) implies that the accumulated cost-to-come of each vertex equals to the 
optimal cost-to-come. This allows us classify the vertices according to their potential of being part 
of the optimal path, and thus to quickly identify the region where the optimal solution is more 
likely to be found. This information can be subsequently used to improve the speed of convergence 
of the standard RRT* algorithm, as well as in order to more efficiently explore the obstacle-free 
space. Three variants of the baseline RRT* algorithm are proposed that take advantage of this 
vertex classification to speed up convergence. 

The organization of the paper is as follows: The problem formulation is given in the next section. 
In Section 3, an overview of the RRT* algorithm is introduced. The fundamental concepts and 
primitive functions used in the RRT* algorithm are explained. In Section 4, each step of the 
proposed approach is explained in detail, along with the pseudo-code of the algorithm and the 
main procedures used in the main algorithm. In Sections 5, simulation results are used to compare 
the solutions of the proposed approach with the well-known RRT* algorithm. In Section 6, several 
variants of the baseline algorithm are presented by using simple vertex rejection techniques and 
improvements are demonstrated by doing extensive simulations in the subsequent section. We 
conclude the paper with some possible extensions for future work. 
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2 Problem Formulation 



2.1 Notation and Definitions 

Let X denote the state space, which is assumed to be an open subset of M. d , where d £ N with 
d > 2. Let the obstacle region and the goal region be denoted by X ^ s and X goa \, respectively. The 
obstacle- free space is defined by Xf YCC — X \ <^obs* Let the initial state be denoted by Xi n jt G Xf ree . 
The neighborhood of a state x £ X is defined as the open ball of radius r 6 M + centered at x, that 
is, B r (x) = {x' £ X : \\x — x'|| < r}. Let <5 = (V, E 1 ) denote a graph, where V and E C V x V 
are finite sets of vertices and edges, respectively. In the sequel, we will use graphs to represent the 
connections between a (finite) set of points selected randomly from X{ rcc . With a slight abuse of 
notation, we will use x to denote both the point in the space X and the corresponding vertex in 
the graph. 

Geometric r-disc graph: Let V C M. d be a finite set, and r > 0. A geometric r-disc graph 
G(V;r) = (V, E) in d dimensions is an undirected graph with vertex set V and edge set E = 
{(u, v ) : u, v £ V and \\u — v\\ < r}. 

Successor vertices: Given a vertex v £ V, the set-valued function succ : (0,v) t- > V C V 
returns the vertices in V that can be reached from vertex v, 

succ(Q,v) := {u G V : (v,u) £ E} 

Predecessor vertices: Given a vertex v € V in a directed graph Q = (V,E), the function 
pred : (Q, v) t- > V' C V returns the vertices in V that are the tails of the edges going into v, 



Parent vertex: Given a vertex v £ V, the function parent : v i— > u returns the unique vertex 
u £ V such that (u, v ) £ E and u £ pred(t/, v). 

Spanning tree: Given the graph Q = (V,E), a spanning tree of Q can be defined such that 
T = (V s , E s ), where V s = V and E s = {(u, v ) : u, v £ V, (u, v) £ E and parent (v) = u}. 

Edge cost value: Given an edge e = (u, v) £ E, the function c:ei->r returns a non-negative 
real number. Then c(n, v) where v £ succ(Q, u) is the cost incurred by moving from u to v. Cost- 
to-come value: Given a vertex v £ V, the function g : v i-> r returns a non-negative real number r, 
which is the cost of the path to v from a given initial state x- m i t £ Xf ree . Let g*(t>) be the optimal 
cost-to-come value of the vertex v. The optimal cost-to-come satisfies the following relationship: 



Each vertex v is associated with two estimates of the optimal cost-to-come value g*(v), namely, 
g(v) (g- value) and lmc(f) (locally minimum cost-to-come estimate, or lmc-value). The lmc(v) is 
the best estimate of the cost-to-come of the vertex v, computed based on the g- value of the vertices 
in the predecessor set pred(w). The lmc-value (also called rhs- value in [13]) is a one-step ahead 
lookahead value based on the g-value and is thus potentially better informed than the g-value of 
the vetrex. The lmc-value satisfies the following relationship 



[min Mepred(gi „ ) (g(u) + c(u,v)), otherwise. 

Heuristic value: Given a vertex v £ V, and a goal region X goa \, the function h : (v, X goa \) t- > r £ 
K returns an estimate of the optimal cost from v to X goa x; it is if v £ X gogi \. It is an admissible 



pred(G,v) := {u £ V : (u,v) £ E} 





Hv = Xinit, 
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heuristic if it never overestimates the actual cost of reaching X goa \. In this paper, we always assume 
an admissible heuristic. It is well known that inadmissible heuristics can be used to speed-up the 
algorithm, but they lead to suboptimal paths [19]. 

Relevant region: Let x* oal 6 X goa _\ be the point in the goal region that has the lowest optimal 
cost-to-come value in X goa i, i.e., x* oal = argmin rg;Vgoai g*(x). The relevant region of Aff ree is the set 
of points x for which the optimal cost-to-come value of x, plus the estimate of the optimal cost 
moving from x to X goa \ is less than the optimal cost-to-come value of x* 1; that is, 



Points that lie in the X re \ have the potential to be part of the optimal path starting at Xi n a and 
reaching X goai . 

Key value: Given a vertex v £ V, the function Key : v i— >■ k returns a real vector k 6 M 2 , whose 
components are k\(v) = min(g(w), lmc(i;)) +h(v) and /^(u) = min(g(w), lmc(v)), respectively. Com- 
ponents of the keys correspond to the f- values and g- values in the A* algorithm, respectively [18]. 

Promising vertices: Let t>* oal £ V be the vertex that has the lowest key value, i.e., t>* oal = 
argmin t)g y nA . goai Key(t;). The promising vertices V pToni C V is the set of vertices that have better 
key value than v* al , that is, 



Priority of vertices: The priority of vertices in the queue is the same as the priority of their 
associated keys, and the precedence relation between keys is determined according to lexicographical 
ordering. Given two keys k, k' £ M?, the Boolean function =<; : (k, k') i-> {False, True} returns True 
if and only if either k\ < k[ or {k\ = k[ and k 2 < k' 2 ), and False otherwise. 

Consistency: A vertex v £ V is called locally consistent if and only if its g- value equals its 
lmc- value [13]. Otherwise, it is an inconsistent vertex. The notion of consistency is very important 
because it allows one to update cost-to-come values of all vertices by propagating the effects of the 
changes in the topology of the graph. This way, an incremental search can reuse information from 
the previous searches, thus speeding up the whole algorithm. The lmc-value always keeps the best 
up-to-date estimate of the cost-to-come value based on the current topology of the graph, whereas 
the g- value keeps an estimate of the cost-to-come value computed from a previous topology of the 
graph. Equality of the g- and lmc-values of a vertex implies that the changes in the topology of 
the graph will not effect the cost-to-come value of that vertex, that is, the topology of the graph is 
consistent with its previous configuration in the locality of the vertex. 

A tree T = (V S ,E S ) is called a consistent tree if and only if all of its promising vertices are 
consistent. 

The g-value of all vertices equals to their respective optimal cost-to-come value if and only if 
all vertices are locally consistent [13]. The g- values have the following form when all vertices are 
locally consistent 



Then, the shortest path from Xmit £ Xf ree to any vertex v £ V can be found by starting at v 
and traversing iteratively from the current vertex u £ V to any of its predecessor u' £ pred(C?,u) 
that minimizes g(n') + c(u',u) (ties can be broken arbitrarily), until Xmit is reached. 



XrA = {X£ Xfrcc : g*(x) + h(x) < g*(x* oal )} 



^prom = {v £ V : Key(v) ~< Key(«* oal )} 
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2.2 Problem Definition 

The proposed RRT* algorithm solves the following motion planning problem: Given a bounded 
and connected open set X C M. d , and the sets Xf ree and X Q bs = X\X{ ree , an d given an initial point 
Xinit G ^frcc an d a goal region X goa \ C Xf ree , find the minimum-cost path connecting Xi n it to the 
goal region X goa \. If no such path exists, then report that no solution is possible. 

3 The RRT # Algorithm - Overview 

A brief description of each function used in the RRT* algorithm is given below. 

Sampling: Sample : N — > X{ me is a function that returns independent, identically distributed 
(i.i.d) samples from X{ iee . 

Nearest neighbor: Nearest is a function that returns a point from a given finite set V, which is 
the closest to a given point x in terms of a given distance function. 

Near vertices: Near is a function that returns n number of points from a given finite set V, 
which is the closest to a given point x in terms of a given distance function. 

Steering: Steer is a function that returns the closest point in a ball centered around a given 
state x to another given point x ne w 

Collision checking: Given two points, the Boolean function ObstacleFree checks whether the 
minimum distance path connecting these two points belongs to Xf rcc . It returns True if the line 
segment is a subset of the Xf ree . 

Tree extension: Extend is a function that extends the nearest vertex of the tree T towards the 
randomly sampled point x ra nd- 

Reducing inconsistency: Given a graph Q = (V, E), a corresponding spanning tree T = (V s , E s ), 
where V s = V and E s C V x V and a goal region X goa \ C Xf ree , the function Reducelnconsistency : 
(Q, T, -^goai) {Qi T') operates on the inconsistent vertices of the tree T iteratively, and continues 
until the tree becomes consistent, that is, all vertices of the tree that are promising (see Section 4) are 
consistent. The Reducelnconsistency function is used to propagate the effects of the topological 
changes in the graph Q as new vertices are added with each iteration. 

A priority queue is used to sort all of the inconsistent vertices of the tree T based on their 
respective key values. The following functions are defined to manage the priority queue. 

Update queue: Given a vertex v € V, the function UpdateQueue changes the content of the 
queue based on the g- and lmc- values of the vertex v. If the vertex v is inconsistent, then it is 
either inserted into the queue or its priority in the queue is updated based on its up-to-date key 
value if it is already inside the queue. Otherwise, the vertex is removed from the queue if it is a 
consistent vertex. 

Find minimum: The function findminQ returns the vertex with the highest priority of all 
vertices in the queue, i.e., the vertex of minimum key value. 

Remove a vertex: Given a vertex v 6 V, the function remove () deletes the vertex v from content 
of the queue. 

Update priority: Given a vertex v £ V, and a key value k, the function update() changes the 
priority of the vertex v in priority queue q, i.e., it reassigns the key value of the vertex v with the 
new given key value k. 

Inserting a vertex: Given a vertex v £ V, and a key k, the function insertQ adds the vertex v 
with the key value k into queue. 
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4 The RRT # Algorithm - Details 



The body of the RRT* algorithm is given in Algorithm 1 and it is similar to the other RRT- variants 
(RRT, RRG, RRT*, etc) with the notable exception that it keeps track of vertex consistency using 
the key values of all current vertices in the graph. One of the important difference between the 
RRT* and RRT* algorithms is that all vertices in the tree computed by the RRT* algorithm have a 
uniform type based on their finite cost-to-come value, whereas in the RRT* algorithm the vertices 
have different types based on their pair of estimates of the cost-to-come value. In the RRT # 
algorithm, each vertex v can be classified in one of the following four categories based on the values 
of its (g(v),lmc(v)) pair. 

• Consistent with finite key value: g(v) < oo, lmc(i;) < oo and g(v) = lmc(v ) 

• Consistent with infinite key value: g(i> ) = oo, lmc(v) = oo 

• Inconsistent with finite key value: g(v) < oo,lmc(f) < oo and g(v) 7^ lmc(f) 

• Inconsistent with infinite g-value and finite lmc-value: g(v ) = oo, lmc(v) < oo 

Vertices in the second category are always non-promising, whereas vertices in the rest of cate- 
gories can be either promising or non-promising. The promising vertices can be used to approximate 
the region X re \ C X{ vee of the free space that may contain the optimal path. 



Algorithm 1: Body of the RRT* Algorithm 


i RRT # (ir init7 X goah X) 


2 


V <- {zi„it}; E <- 0; 


3 




4 


for i = 1 to N do 


5 




Zrand <- Sample (i); 


6 




Q «- Extend(^,x rand ); 


7 




ReduceInconsistency(C7, X soa ,i); 


8 


(V,E)<-g; E'<-0; 


9 


foreach x G V do 


10 




E' <- E'U {(parent (x),x)} 


11 


return T = (V, E') 



The algorithm starts by adding the initial point Xinit into the vertex set of the underlying graph. 
Then, it incrementally grows the graph in Xf ree by sampling a random point x ran d from X{ TCC and 
extending some parts of the graph towards a; ran d. Later, the Reducelnconsistency procedure, 
which is provided in Algorithm 3, propagates the new information due to the extension across the 
whole graph in order to improve the estimate of the cost-to-come value of the promising vertices 
in the graph. All computations due to the sampling and extension steps, followed by information 
propagation (Lines 4-7 of Algorithm 1), form a single iteration of the algorithm. The process is 
repeated for a given fixed number of iterations, and the consistent spanning tree of the final graph 
is returned at the end. 

The key difference between the RRT* algorithm and other RRT-variants is that a unique 
consistent spanning tree of the graph is maintained at the end of the each iteration of the algorithm. 
Since this tree is consistent, it contains information of the lowest-cost path, which can be achieved on 
the current graph, for each promising vertex of the graph. In addition, the g-value of the promising 
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vertices equals to their respective optimal cost-to-come value that can be achieved through the 
edges of the tree. Therefore, each new vertex is initialized with the minimum possible estimate 
of its respective optimal cost-to-come value during extension (since all of its promising neighbor 
vertices have the lowest g- value), and this estimate keeps improving to the best possible value 
whenever new information becomes available on any part of the graph. Hence, the g-value of each 
promising vertex of the graph converges to its optimal cost-to-come value very quickly. 



Algorithm 


2: Extend Procedure for RRT* Algorithm 


i Extend(G,x) 




2 


{V,E)<- 


G; 


E' <- 0; 


3 


X 


nearest <- Nearest(£, x); 


4 


X 


new 


4 Steer(x neares t , x) , 




if ObstacleFree(x ncarost ,x ncw ) then 


6 




g(Xnew) <- OO; 


7 




lmc(a;new 


) — S(^nearest) H~ c (^nearest 1 ^ncw): 


8 




parent (x ncw ) x ncarcs t, 


9 




^near 


<- Near(£,x ncw , \V\); 


10 




foreach 


^near ^ ^near 


11 






if ObstacleFree(x noar , x now ) then 


12 








if lmc(x ncw ) > g(^ncar) c(x ncar , ^ncw) then 


13 










lmc(x ncw ) — S(^near) H~ c(x ncar , X ncw ) , 


14 










parent (x ncw ) — ^ncar^ 


15 








E 4 E U { (x nea r •> ^new) •> (*^new 5 ^near) }i 


16 




V «- 


VU{x ncw }; 


17 




E <- EU E'; 


18 




Updat eQueue (x ncw ) ; 


19 


return Q' <— 


(V,E) 



The Extend procedure for the RRT* algorithm is given in Algorithm 2. During each iteration, 
the Extend procedure tries to extend the graph towards the randomly sampled point x ran d £ Xfcee- 
First, the closest vertex in the graph x nea rest is found in Line 3, then x nea rest is steered towards the 
randomly sampled point x ran( j in the next line. If the line segment connecting the steered point 
x ncw and ^nearest is feasible, then the new point x ncw is prepared for inclusion to the vertex set of 
the graph. First, its cost-to-come estimate, i.e., the g-value and lmc- values, and the parent vertex 
are initialized by using information of the nearest vertex x nea rest- Then, a local search is performed 
in some neighborhood of x new , i. e., the set of vertices returned by the Near procedure, in order to 
find the local minimum cost-to-come estimate value in Lines 10-15 and the corresponding parent 
vertex. The new vertex x new and all extensions resulting in feasible trajectories are added to the 
vertex and edge set of the graph in Lines 16-17, respectively. In the end, the new vertex is decided 
to be inserted in the priority queue or not based on its consistency in the UpdateQueue procedure. 

Inclusion of each new vertex may result in an inconsistent vertex in the graph if a finite lmc- value 
is achieved. Therefore, consistency of the spanning tree needs to be checked, and appropriate oper- 
ations must be performed in order to make it consistent, if necessary. The Reducelnconsistency 
procedure, which is provided in Algorithm 3, is called to make the spanning tree consistent by 
operating on the inconsistent and promising vertices of the graph, iteratively. It simply pops the 
most promising inconsistent vertex from the priority queue, if there are any, and this inconsistent 
vertex is made consistent by assigning its lmc-value to its g-value. Then, its new g-value informa- 
tion is propagated among its neighbors in order to improve their lmc- values in Lines 7-11. However, 
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Algorithm 


3: Reducelnconsistency Procedure 


i ReduceInconsistency(C* .A'goai) 


2 


while q.findminQ -< Key(x* oal ) do 


3 




X 


= q.findminQ; 


4 




g 


(x) 


= lmc(x); 


5 




q.delete(x); 


6 




foreach s € succ(C*,x) do 


7 






if lmc(s) > g(x) + c(x, s) then 


8 








parent (s) = x; 


9 








lmc(s) = g(x) + c(x, s); 


10 








UpdateQueue(s); 



Algorithm 4: Auxiliary Procedures 


i Initialize^) 


2 


g(x) «- oo; 


3 


lmc(x) <— oo; 


4 


parent (x) <— 0; 


5 UpdateQueue(x) 


6 


if g(x) ^ lmc(x) and 165 then 


7 


q.update(x, Key(x)); 


8 


else if g(x) ^ lmc(x) and x ^ q then 


9 


q.insert{x, Key(x)); 


10 


else if g(x) = lmc(x) and x e g then 


11 


g.ddeie(x); 


12 Key(x) 


13 


5min = min(g(x), lmc(x)); 


14 


/ = ffmin +h(x); 


15 


return key = (/,g mi „); 



this information propagation may also cause some vertices to be inconsistent; therefore, all result- 
ing inconsistent vertices are inserted in the priority queue as well. This process continues until a 
consistent spanning tree is computed, that is, there is no inconsistent promising vertex left in the 
priority queue. 

5 Numerical Simulations 1 

The RRT* algorithm was developed in C++ and run on a computer with a 2.40 GHz pro- 
cessor and 12GB RAM running the Ubuntu 11.10 Linux operating system. A Fibonacci heap 
was implemented as priority queue to store inconsistent vertices during the search [4]. Exten- 
sive simulations were run to compare the performance of the RRT* algorithm with the RRT* 
algorithm, whose C implementation is available to download from the RRT* authors' website 
(http : //sertac . scripts .mit . edu/rrtstar/). 

Both RRT* and RRT* algorithms were run on three different problem types with the same 
sample sequence in order to demonstrate the difference in their behavior while growing the tree. All 
problems tested require finding an optimal path in a square environment minimizing the Euclidean 
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path length. The heuristic value of a vertex is the Euclidean distance from the vertex to the goal. In 
the first problem type, there are no obstacles in the environment, whereas there are some box-like 
obstacles in the second and third problem types. In the third problem type, the environment is 
more cluttered than the one in the second problem type, containing many widely distributed small 
obstacles. 

For the first problem type, the trees computed by both algorithms at different stages are shown 
in Figure 1. The initial state is plotted as a yellow square and the goal region is shown in blue with 
magenta border (upper right). The minimal-length path is shown in red. As shown in Figure 1, 
the best path computed by the RRT* algorithm converges to the optimal path. As mentioned 
earlier, one of the important differences between the RRT* and RRT* algorithms is that the latter 
classifies the vertices in one of the following four categories based on the values of its (g{v), lmc(f )) 
pair: Consistent with finite key value (shown in green), consistent with infinite key value (shown 
in black), inconsistent with finite key value (shown in blue), and inconsistent with infinite g- value 
and finite lmc- value (shown in red). 

Since only the points in the relevant region X re \ have the potential to be part of the optimal 
path, the RRT* algorithm tries to approximate X re \ with the set of promising vertices V^rom and 
tends to stop rewiring the parts of the tree which lie outside of the X re \ as iterations go to infinity. 
As seen in Figure 1, for this particular scenario, X re \ is an elliptic region, which is much smaller 
than the whole Xf ree . Therefore, uniform random sampling on X{ Tee results in too many vertices 
of different types (green, black, red, and blue vertices) outside of the relevant region during the 
search. The estimate of X re \ can be used to implement more intelligent sampling strategies, if 
needed, although this possibility was not pursued in this paper, where all sampling was uniform. 
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Figure 1: The evolution of the tree computed by RRT* and RRT* algorithms is shown in (a)-(d) 
and (e)-(h), respectively. The configuration of the trees (a), (e) is at 250 iterations, (b), (f) is at 
500 iterations, (c), (g) is at 2500 iterations, and (d), (h) is at 25000 iterations. 
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(a) (b) 

Figure 2: The change in the cost of the best paths computed by RRT* and RRT* algorithms, and 
the variance in the trials are shown in (a) and (b), respectively. 



In the second problem type, the same experiment was carried out and both algorithms were 
run in an environment with several obstacles. The configuration of the trees for both the RRT* 
and RRT* algorithms at different stages are shown in Figure 3. 
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Figure 3: The evolution of the tree computed by RRT* and RRT* algorithms is shown in (a)-(d) 
and (e)-(h), respectively. The configuration of the trees (a), (e) is at 250 iterations, (b), (f) is at 
500 iterations, (c), (g) is at 2500 iterations, and (d), (h) is at 25000 iterations. 
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(1)) 



Figure 4: The change in the cost of the best paths computed by RRT* and RRT* algorithms and 
the variance in the trials are shown in (a) and (b), respectively. 



In the third problem type, both algorithms were run in a more cluttered environment, where 
there are many different homotopy classes containing the local minimum solution for the problem. 
As shown in Figure 5, both algorithms switch between paths which have locally best cost, eventually 
converging to the optimal solution. 
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Figure 5: The evolution of the tree computed by RRT* and RRT* algorithms is shown in (a)-(d) 
and (e)-(h), respectively. The configuration of the trees (a), (e) is at 250 iterations, (b), (f) is at 
500 iterations, (c), (g) is at 2500 iterations, and (d), (h) is at 25000 iterations. 
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(a) (b) 

Figure 6: The change in the cost of the best paths computed by RRT* and RRT* algorithms and 
the variance in the trials are shown in (a) and (b), respectively. 



Finally, in the fourth problem type, both algorithms were run in a obstacle-free environment 
where there are different cost zones. The cost coefficient of each zone from top to bottom is 1.5, 
0.75, 2.5, 0.75, and 1.5, respectively and 1 elsewhere. As seen in Figure 7, both algorithms compute 
the optimal path which has longer segments in low-cost zones. 




(e) (f) (g) (h) 



Figure 7: The evolution of the tree computed by RRT* and RRT* algorithms is shown in (a)-(d) 
and (e)-(h), respectively. The configuration of the trees (a), (e) is at 250 iterations, (b), (f) is at 
500 iterations, (c), (g) is at 2500 iterations, and (d), (h) is at 25000 iterations. 
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(a) (b) 

Figure 8: The change in the cost of the best paths computed by RRT* and RRT* algorithms, and 
the variance in the trials are shown in (a) and (b), respectively. 



6 Variants of the RRT^ Algorithm 

Too many non-promising vertices are included in the tree computed by the RRT* algorithm as 
observed in the previous simulations. This is owing to the fact that the RRT* algorithm includes 
all new vertices in the graph regardless of their type. A simple vertex selection criterion can be 
used in the Extend procedure in order to prevent the algorithm from growing the tree towards the 
region outside X re \. However, being over-selective on vertex inclusion may degrade the performance 
of the algorithm - and thus lead to a suboptimal solution - since the cost-to-come value of all 
vertices, which is used to decide if a new vertex is promising or not, is an estimate of the optimal 
one. In this section, we propose three variants of the baseline RRT* algorithm. 

RRTy X : In the first variant, which is given in Algorithm 5, if a new vertex happens to be consistent 
with infinite key value (black vertex), it is not included in the graph. This situation can 
happen if all of the neighbor vertices of the new vertex happen to be inconsistent with infinite 
g- value and finite lmc- value (red vertices). First, the estimates of the cost-to-come- value of 
the new vertex x new are initialized with infinite cost, and its parent vertex is set to 'null' 
in Line 6. Then, a better value for the lmc-value of the new vertex is searched among its 
neighbor vertices. During this search, the parent of the new vertex remains unassigned only 
if there are no any neighboring vertices with finite g-value. 

RRTy 2 : In the second variant, the algorithm becomes more selective on vertices to be added to the 
graph and the "parent (x ncw ) ^ A Key(parent(x nC w)) -< Key(x* oal )" condition is checked 
in Line 14. Simply, a new vertex is included to the graph only if its parent is a promising 
vertex. 

RRTy 3 : Lastly, the third variant is most selective on vertex for inclusion and Key(x new ) -< 
Key(x* oal ) condition is checked, that is, only promising new vertices are included in the graph. 
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Algorithm 


5: Extend Procedure for RRT^Algorithm 


i Extend(Q ,x) 




2 


{V,E)<- 


G; 




3 


X 


nearest <- Nearest(£, x); 


4 


X 


new 


4 Steer (x nearcs t , x) , 




if ObstacleFree(ir noar cst, x ncw ) then 


6 




Initialize (x now ) ; 


7 




^near 


<- Near(£,x ncw , \V\); 


8 




foreach 


^near ^ *^-near ^0 


9 






if ObstacleFree(x nC ar, a^new) then 


10 








if lmc(x ncw ) ]> g(^ncar) H~ c(x ncar , x ncw ) then 












lmc(x ncw ) — §(^ncar) c(x ncar , X ncw ) , 


12 










parent (x ncw ) — ^ncari 


13 








^ ^ E U {(^neari <^new)j (^new;^near)}i 


14 




if parent (x now ) ^ then 


15 






V <- 


V U {x ncw }; 


16 






E 


1 <- 


E\JE'- 


17 






Updat e Queue (x now ) ; 


18 


return (/' <— 


(V,E) 















7 Numerical Simulations 2 

The same experiments as before were carried out for the three variants of the RRT* algorithm. As 
seen in the figures below, all variants successfully prevent the inclusion of vertices which lie in the 
unfavorable regions of the search space. As seen in Figures 9(e), 12(e), 15(e), and 18(e), the RRT# X 
algorithm does not include any black vertices in the tree (these are the vertices that are consistent 
with infinite key value, hence non-promising), but still computes a solution to the problem, which 
is as good as the one computed by the RRT* and RRT* algorithms. However, there are still 
many red (i.e., non-promising and inconsistent with infinite g- value and finite lmc- value) vertices 
included in the tree. This is owing to the fact that they are never made consistent until the last 
iteration, since they mostly lie outside of X re \. Therefore, they remain in the priority queue and 
need to be sorted during each iteration. This makes the Reducelnconsistency procedure slower. 
In the RRT* 2 algorithm, the number of red vertices included into the tree is reduced by simply 
enforcing to have a promising parent vertex for the new vertex that is considered for extension. 
Red vertices are mostly included into the branches of the tree that are formed outside of the X re \ 
during exploration phase. As seen in Figures 9(v), 12(v), 15(v), and 18(v), the RRT* 2 algorithm 
tends not to include vertices into the branches of the tree which are very far away from the optimal 
solution. Lastly, the RRTy 3 algorithm includes a new vertex into the tree only if it is a promising 
one. Therefore, all vertices in the tree, other than the goal vertices, are either green or blue, which 
are located around the boundary of X re \. 

The convergence rate and variance in the computation of the best path for all algorithms are 
shown in Figures 11, 14, 17, and 20. Since this is a two-dimensional problem, the optimal path 
for each problem type can be computed visually and the cost of the paths for each algorithm is 
normalized with respect to the cost of the optimal solution. The ratio of the cost of the best path 
over the optimal cost for the RRT*, RRT # , RRT# 1; RRT* 2 , and RRT* 3 algorithms is shown in 
red, blue, green, magenta, and black colors, respectively. 
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(a) (b) (c) (d) 




(e) (v) 



Figure 9: The evolution of the tree computed by RRT^ and RRT* 2 algorithms is shown in (a)-(e) 
and (i)-(v), respectively. The configuration of the trees (a), (i) is at 250 iterations, (b), (ii) is at 
500 iterations, (c), (iii) is at 2500 iterations, (d), (iv) is at 10000 iterations, and (e), (v) is at 25000 
iterations. 
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(a) (b) (c) 




(d) (e) (f) 

Figure 10: The evolution of the tree computed by RRTy 3 algorithm is shown in (a)-(f). The 
configuration of the trees in (a) is at 250 iterations, in (b) is at 500 iterations, in (c) is at 2500 
iterations, in (d) is at 5000 iterations, in (e) is at 10000 iterations, and in (f) is at 25000 iterations. 
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Figure 11: The change in the cost of the best paths computed by RRT*, RRT*, and its variant 
algorithms and the variance in the trials are shown in (a) and (b), respectively. 
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Figure 12: The evolution of the tree computed by RRTy 1 and RRTj> 2 algorithms is shown in (a)-(e) 
and (i)-(v), respectively. The configuration of the trees (a), (i) is at 250 iterations, (b), (ii) is at 
500 iterations, (c), (iii) is at 2500 iterations, (d), (iv) is at 10000 iterations, and (e), (v) is at 25000 
iterations. 
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(d) (e) (f) 

Figure 13: The evolution of the tree computed by RRTy 3 algorithm is shown in (a)-(f). The 
configuration of the trees in (a) is at 250 iterations, in (b) is at 500 iterations, in (c) is at 2500 
iterations, in (d) is at 5000 iterations, in (e) is at 10000 iterations, and in (f) is at 25000 iterations. 




(a) (b) 

Figure 14: The change in the cost of the best paths computed by RRT*, RRT*, and its variant 
algorithms and the variance in the trials are shown in (a) and (b), respectively. 
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Figure 15: The evolution of the tree computed by RRTy 1 and RRTj> 2 algorithms is shown in (a)-(e) 
and (i)-(v), respectively. The configuration of the trees (a), (i) is at 250 iterations, (b), (ii) is at 
500 iterations, (c), (iii) is at 2500 iterations, (d), (iv) is at 10000 iterations, and (e), (v) is at 25000 
iterations. 
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(a) (b) (c) 




configuration of the trees in (a) is at 250 iterations, in (b) is at 500 iterations, in (c) is at 2500 
iterations, in (d) is at 5000 iterations, in (e) is at 10000 iterations, and in (f) is at 25000 iterations. 
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Figure 17: The change in the cost of the best paths computed by RRT*, RRT*, and its variant 
algorithms and the variance in the trials are shown in (a) and (b), respectively. 
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(a) 



(b) 



(c) 
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Figure 18: The evolution of the tree computed by RRT^ and RRT* 2 algorithms is shown in (a)-(e) 
and (i)-(v), respectively. The configuration of the trees (a), (i) is at 250 iterations, (b), (ii) is at 
500 iterations, (c), (iii) is at 2500 iterations, (d), (iv) is at 10000 iterations, and (e), (v) is at 25000 
iterations. 
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(d) (e) (f) 

Figure 19: The evolution of the tree computed by RRTy 3 algorithm is shown in (a)-(f). The 
configuration of the trees in (a) is at 250 iterations, in (b) is at 500 iterations, in (c) is at 2500 
iterations, in (d) is at 5000 iterations, in (e) is at 10000 iterations, and in (f) is at 25000 iterations. 




(a) (b) 

Figure 20: The change in the cost of the best paths computed by RRT*, RRT*, and its variant 
algorithms and the variance in the trials are shown in (a) and (b), respectively. 
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A Monte-Carlo study was performed in order to compare the convergence rate and variance in 
the trials of all algorithms in a high dimensional search space. All algorithms were run up until 
4 million iterations 100 times in a 5-dimensional search space for Problem types 1 and 2. In the 
second problem type, several 5-dimensional hypercubes of different size were randomly placed in 
the environment in order to represent obstacles. As shown in Figures 21 and 22, the RRTy 2 and 
RRTy 3 algorithms find the solution in a similar amount of time, and they are faster than the 
other algorithms. In addition, they compute solutions of lower cost than the other algorithms with 
smaller variance in the trials. 




(a) (b) 

Figure 21: The change in the cost of the best paths computed by RRT*, RRT*, and its variant 
algorithms and the variance of the trials is shown in (a) and (b), respectively (problem type 1, 5D 
search space). 




(a) (b) 

Figure 22: The change in the cost of the best paths computed by RRT*, RRT*, and its variant 
algorithms and the variance of the trials are shown in (a) and (b), respectively (problem type 2, 
5D search space). 



The execution times of all algorithms were also compared. Results of the RRT*, RRT*p 
RRTy 2 , and RRTy 3 are plotted in blue, green, magenta, and black, respectively. All algorithms 
were run in a 2D and a 5D environment with no obstacles for up to 750,000 and 4,000,000 iterations, 
respectively. The execution time of the RRT* and its variant algorithms is normalized over that 
of the RRT* algorithm and is plotted versus the number of iterations averaged over 50 trials for the 
2D search space in Figure 23(a). A similar plot is also created for 100 trials in the 5D search space 
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8 Conclusion 

In this paper, a new incremental sampling-based algorithm, denoted by RRT* is presented, which 
offers asymptotically optimal solutions for solving motion planning problems. The RRT* algorithm 
relies heavily on the random geometric graph data structure and the RRG algorithm [20], which 
is also known to have asymptotic optimality properties. A bottleneck of optimal sampling-based 
algorithms is the slow convergence to the optimal solution, although sampling-based algorithms 
are capable of finding a feasible solution, often almost in real-time. By incorporating consistency 
information of all current vertices in the tree (essentially by comparing the current cost-to-come 
values of the vertices with the cost-to-come values via one of the neighboring vertices) we can have 
more informed estimates of the optimal values of the potential paths, thus speeding up convergence. 
Furthermore, once a feasible path has been found, vertex consistency can be used to estimate the 
region where the optimal solution should be found. This results in an initial convergence rate that 
is better than the one of the RRT* algorithm. 

We have also introduced three variants to improve the convergence rate of the baseline RRT* 
algorithm by implementing two key features: preventing the expansion of the tree towards unfavor- 
able regions in search space, and propagating new information throughout the tree in an efficient 
way. The first feature allows us to limit the number of vertices in the tree, thus resulting to the 
algorithm running faster. The second feature allows us to compute solutions with a less number of 
vertices in the tree since any new information is exploited to the highest degree. As a result, the 
convergence rate of the baseline RRT* can be improved significantly. Extensive numerical results 
have verified these observations in several simulation scenarios. 

The work in this paper can be extended in several directions. First, a thorough theoretical anal- 
ysis is warranted in order to provide strict bounds on the convergence rate of RRT*. Second, since 
RRT* decomposes the vertex set into "promising" and "non- promising" ones, smarter sampling 
strategies can be developed to exploit this information. It is also crucial for the algorithm to reach 
the target set as early as possible in order to converge to the optimal solution faster. In that respect, 
a bi-directional version of the RRT* (like the RRT-connect in [14]) can be developed in order to 
shorten the first time-to-connect to the goal set. Also, a parallel version of the algorithm could be 
implemented by running the Extend and Reducelnconsistency procedures as separate threads. 
A possible implementation would be to have multiple threads implementing the Extend procedure 
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and single thread implementing the Reducelnconsistency. Finally, the algorithm can be modified 
to solve motion planning problems for vehicles with complex dynamics (ground vehicles, aircraft, 
helicopters etc) by implementing specific local steering functions. 
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